/**
 * @author luo
 * @brief
 * @version 1.0
 * @date 2023-09-22
 *
 */

#include "transform_algorithm.h"

double deg2rad(double deg) {
    return deg * M_PI / 180;
}

/*****弧度转化为角度*****/


double rad2deg(double rad) {
    return rad * 180 / M_PI;
}

void joint2rad(std::vector<double> &joint_in_degree) {
    for (auto &joint: joint_in_degree) {
        joint = joint * M_PI / 180;
    }
}




Matrix3d euler2rot(array<double, 3> &zyx) {
    Matrix3d r_m;
    r_m = AngleAxisd(zyx[0], Vector3d::UnitZ()) * AngleAxisd(zyx[1], Vector3d::UnitY()) *
          AngleAxisd(zyx[2], Vector3d::UnitX());
    return r_m;
}

Matrix3d rpy2rot(array<double, 3> rpy) {
    Matrix3d rotation_matrix;
    AngleAxisd rollAngle(AngleAxisd(rpy[0], Vector3d::UnitX()));
    AngleAxisd pitchAngle(AngleAxisd(rpy[1], Vector3d::UnitY()));
    AngleAxisd yawAngle(AngleAxisd(rpy[2], Vector3d::UnitZ()));
    rotation_matrix = yawAngle * pitchAngle * rollAngle;
    return rotation_matrix;
}

/*****旋转向量转化为旋转矩阵*****/
//Matrix3d rotvec2rotMatrix(array<double, 3> rot_vec) {
//    Matrix3d rotMatrix;
//    Vector3d rotvec(rot_vec[0], rot_vec[1], rot_vec[2]);
//    double theta = rotvec.norm();
//    rotvec = rotvec / theta;
//    Matrix3d rotvecMat, cosMat, rot;
//    rotvecMat << 0, -rotvec(2), rotvec(1), rotvec(2), 0, -rotvec(0), -rotvec(1), rotvec(0), 0;
//    rotvecMat = sin(theta) * rotvecMat;
//    rot = rotvec * rotvec.transpose();
//    rot = (1 - cos(theta)) * rot;
//    cosMat = cos(theta) * Matrix3d::Identity(3, 3);
//    rotMatrix = cosMat + rot + rotvecMat;
//    return rotMatrix;
//}

Matrix3d rotvec2rotMatrix(Vector3d rotvec) {
    Matrix3d rotMatrix;
    //罗德里格斯公式R=cos(theta)I+(1-cos(theta)nnT+sin(theta)n^,需要排除0向量
    double theta = rotvec.norm();
    if (theta > 1e-5) {
        rotvec = rotvec / theta;
        Matrix3d rotvecMat, cosMat, rot;
        rotvecMat << 0, -rotvec(2), rotvec(1), rotvec(2), 0, -rotvec(0), -rotvec(1), rotvec(0), 0;
        rotvecMat = sin(theta) * rotvecMat;
        rot = rotvec * rotvec.transpose();
        rot = (1 - cos(theta)) * rot;
        cosMat = cos(theta) * Matrix3d::Identity(3, 3);
        rotMatrix = cosMat + rot + rotvecMat;
    } else {
        rotMatrix.setIdentity();
    }
    return rotMatrix;
}

#pragma endregion

/*****旋转矩阵转化为旋转向量*****/
#pragma region rotMatrix2rotvec

Vector3d rotMatrix2rotvec(Matrix3d rotMatrix) {
    Vector3d rotvec;
    Matrix3d rot;
    AngleAxisd aa(rotMatrix);
    rotvec = aa.angle() * aa.axis();
    //后面的算法没有对theta近似0的情况进行讨论，算法存在缺陷
//    double theta = acos((rotMatrix.trace() - 1) / 2);
//    rot = (rotMatrix - rotMatrix.transpose()) / (2 * sin(theta));
//    rotvec(0) = theta * rot(7);
//    rotvec(1) = theta * rot(2);
//    rotvec(2) = theta * rot(3);
    // cout << rotvec.transpose() << endl;
    return rotvec;
}

#pragma endregion

/*****旋转向量转化为RPY*****/
#pragma region rotvec2rpy

array<double, 3> rotvec2rpy(double rx, double ry, double rz) {
    array<double, 3> rpy;
    Vector3d rot_vec = {rx, ry, rz};

    double theta = rot_vec.norm();
    rot_vec = rot_vec.normalized();
    if (theta < 1e-5) {
        rpy[0] = 0;
        rpy[1] = 0;
        rpy[2] = 0;
    } else {
        AngleAxisd aa(theta, rot_vec);
        Vector3d euler_angle = aa.matrix().eulerAngles(2, 1, 0);
        rpy[0] = euler_angle(2);
        rpy[1] = euler_angle(1);
        rpy[2] = euler_angle(0);
    }
    return rpy;
}

#pragma endregion

/*****RPY角转化为旋转向量*****/
#pragma region rpy2rotvec

array<double, 3> rpy2rotvec(double rx, double ry, double rz) {
    array<double, 3> rotvec;
    AngleAxisd roll_angle(AngleAxisd(rx, Vector3d::UnitX()));
    AngleAxisd pitch_angle(AngleAxisd(ry, Vector3d::UnitY()));
    AngleAxisd yaw_angle(AngleAxisd(rz, Vector3d::UnitZ()));
    AngleAxisd rotate_vec;
    rotate_vec = yaw_angle * pitch_angle * roll_angle;
    double theta = rotate_vec.angle();
    rotvec[0] = theta * rotate_vec.axis()(0);
    rotvec[1] = theta * rotate_vec.axis()(1);
    rotvec[2] = theta * rotate_vec.axis()(2);
    return rotvec;
}

Eigen::Quaterniond rotvec2quat(Vector3d &rot_vec) {
    double theta = rot_vec.norm();
    Eigen::Quaterniond quat;
    if (theta > 1e-5) {
        Vector3d axis = rot_vec / theta;
        quat = {cos(theta / 2), sin(theta / 2) * axis[0], sin(theta / 2) * axis[1], sin(theta / 2) * axis[2]};
    } else {
        quat = {1, 0, 0, 0};
    }
    return quat;
}

Vector3d quat2rot_vec(Eigen::Quaterniond quat) {
    AngleAxisd aa(quat);
    Vector3d rot_vec(aa.angle() * aa.axis());
    return rot_vec;
}

Matrix4d rot_vec_pose2mat(vector<double> &pose) {
    Matrix4d mat;
    Matrix3d rot = rotvec2rotMatrix({pose[3], pose[4], pose[5]});
    mat << rot(0, 0), rot(0, 1), rot(0, 2), pose[0],
            rot(1, 0), rot(1, 1), rot(1, 2), pose[1],
            rot(2, 0), rot(2, 1), rot(2, 2), pose[2],
            0, 0, 0, 1;
    return mat;
}

Matrix4d rpy_vec_pose2mat(vector<double> &pose) {
    Matrix3d rot = rpy2rot({pose[3], pose[4], pose[5]});
    Matrix4d mat;
    mat << rot(0, 0), rot(0, 1), rot(0, 2), pose[0],
            rot(1, 0), rot(1, 1), rot(1, 2), pose[1],
            rot(2, 0), rot(2, 1), rot(2, 2), pose[2],
            0, 0, 0, 1;
    return mat;
}

vector<double> mat2rot_vec_pose(Matrix4d mat) {
    vector<double> pose(6, 0);
    pose[0] = mat(0, 3);
    pose[1] = mat(1, 3);
    pose[2] = mat(2, 3);
    Matrix3d rot;
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            rot(i, j) = mat(i, j);
        }
    }
    Vector3d rot_vec = rotMatrix2rotvec(rot);
    pose[3] = rot_vec(0);
    pose[4] = rot_vec(1);
    pose[5] = rot_vec(2);
    return pose;
}

Matrix<double, 6, 6> transform_adjoint_matrix(Matrix4d &mat) {
    Matrix<double, 6, 6> adjoint_mat;
    Matrix3d rot_mat, p_skew, pr;
    rot_mat << mat(0, 0), mat(0, 1), mat(0, 2),
            mat(1, 0), mat(1, 1), mat(1, 2),
            mat(2, 0), mat(2, 1), mat(2, 2);
    p_skew << 0, -mat(2, 3), mat(1, 3),
            mat(2, 3), 0, -mat(0, 3),
            -mat(1, 3), mat(0, 3), 0;
    pr = p_skew * rot_mat;
    adjoint_mat.setZero();
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            adjoint_mat(i, j) = rot_mat(i, j);
            adjoint_mat(i, j + 3) = pr(i, j);
            adjoint_mat(i + 3, j + 3) = rot_mat(i, j);
        }
    }
    return adjoint_mat;
}

vector<double> rpy_pose2rot_vec_pose(array<double, 6> &rpy_pose) {
    array<double, 3> rotvec = rpy2rotvec(rpy_pose[3], rpy_pose[4], rpy_pose[5]);
    vector<double> pose(6, 0);
    for (int i = 0; i < 3; i++) {
        pose[i] = rpy_pose[i];
        pose[i + 3] = rotvec[i];
    }
    return pose;
}

vector<double> rpy_pose2rot_vec_pose(vector<double> &rpy_pose) {
    array<double, 3> rotvec = rpy2rotvec(rpy_pose[3], rpy_pose[4], rpy_pose[5]);
    vector<double> pose(6, 0);
    for (int i = 0; i < 3; i++) {
        pose[i] = rpy_pose[i];
        pose[i + 3] = rotvec[i];
    }
    return pose;
}

vector<double> rot_vec_pose2rpy_pose(vector<double> &rv_pose) {
    array<double, 3> rpy = rotvec2rpy(rv_pose[3], rv_pose[4], rv_pose[5]);
    vector<double> rpy_pose = {rv_pose[0], rv_pose[1], rv_pose[2], rpy[0], rpy[1], rpy[2]};
    return rpy_pose;
}

////// 四元数转换为旋转矩阵
/// https://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/index.htm
Eigen::Matrix3d Quat2RotMat(const Eigen::Quaterniond &q) {
    return q.matrix();
}

Quaterniond matrix2quat(const Matrix3d rot) {
    double tr, qw, qx, qy, qz;
    double m00 = rot(0, 0);
    double m01 = rot(0, 1);
    double m02 = rot(0, 2);
    double m10 = rot(1, 0);
    double m11 = rot(1, 1);
    double m12 = rot(1, 2);
    double m20 = rot(2, 0);
    double m21 = rot(2, 1);
    double m22 = rot(2, 2);

    tr = m00 + m11 + m22;
    if (tr > 0) {
        double sqdip1 = std::sqrt(tr + 1.0) * 2;
        qw = 0.25 * sqdip1;
        qx = (m21 - m12) / sqdip1;
        qy = (m02 - m20) / sqdip1;
        qz = (m10 - m01) / sqdip1;
    } else if ((m00 > m11) && (m00 > m22)) {
        double sqdip1 = std::sqrt(1.0 + m00 - m11 - m22);
        qx = 0.5 * sqdip1;
        if (sqdip1 != 0)
            sqdip1 = 0.5 / sqdip1;
        qw = (m21 - m12) * sqdip1;
        qy = (m01 + m10) * sqdip1;
        qz = (m02 + m20) * sqdip1;
    } else if (m11 > m22) {
        double sqdip1 = std::sqrt(1.0 + m11 - m00 - m22);
        qy = 0.5 * sqdip1;
        if (sqdip1 != 0)
            sqdip1 = 0.5 / sqdip1;
        qw = (m02 - m20) * sqdip1;
        qx = (m01 + m10) * sqdip1;
        qz = (m12 + m21) * sqdip1;
    } else {
        double sqdip1 = std::sqrt(1.0 + m22 - m00 - m11);
        qz = 0.5 * sqdip1;
        if (sqdip1 != 0)
            sqdip1 = 0.5 / sqdip1;
        qw = (m10 - m01) * sqdip1;
        qx = (m02 + m20) * sqdip1;
        qy = (m12 + m21) * sqdip1;
    }
    Quaterniond quat(qw, qx, qy, qz);
    quat.normalize();
    return quat;

}

